#include "defines.h"
#include "configCANbus.h"

// Code sharing with Interface board for testing.
#ifdef BOARD_TYPE
#include "definesRmat.h"		// only to support flags, should be moved with receive_servo_inputs() MJC
#endif

#include <string.h>

void send_servo_outputs(void);		// Autopilot send servo data to interface
void receive_servo_inputs(void);	// Autopilot recieve servo data from interface

void ResetAllCAN1Tx(void);
void ResetAllCAN1Rx(void);


#ifdef BOARD_TYPE
// Servo data buffers for autopilot canbus
unsigned int ServoOutBuff[8];
unsigned int ServoInBuff[8];
#endif

//---------------------------------------------------------------------

unsigned int RxBytes = 0;

volatile unsigned int TxMsgCount	= 0xFFFF;  	// Counter for which servo tx message is being done
volatile unsigned int TxDataCount	= 0x0000;  	// Counter for data send

void init_CAN1(void);
void config_CAN1_RX_SERVO(void);
void config_CAN1_RX_COPRO(void);
void config_CAN1_TX_SERVO(void);
void config_CAN1_TX_DATA(void);

void send_flight_data(void);	// Probably needs to be moved to a header. MJC

void init_canbus_comms(void)
{
	init_CAN1();
	config_CAN1_RX_SERVO();
	config_CAN1_RX_COPRO();
	config_CAN1_TX_SERVO();
	config_CAN1_TX_DATA();

 //Change to Operation Mode from Configuration Mode
 C1CTRLbits.REQOP = 0;
 while(C1CTRLbits.OPMODE != 0);//Wait for CAN1 mode change from Configuration Mode to normal mode 

 //Enable transmission
 TxMsgCount = 0;	// Clear the servo Tx message counter to indicate that CAN is ready

};


//-------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------
				//init CAN1
//-------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------
void init_CAN1(void)
{

// Code sharing with Interface board for testing.
// This code does not really need to be done.
// The CANbus module takes over the pins when in normal operation mode.
#ifdef BOARD_TYPE
	TRISF = TRISF |= 0x0002;			// CAN 1 TX set to output	
	TRISF = TRISF &= 0xFFFE;			// CAN 1 RX set to input
#else
	TRISF = TRISF |= 0x0002;			// CAN 1 TX set to output	
	TRISF = TRISF &= 0xFFFE;			// CAN 1 RX set to input
#endif

//--------------------------------------------------------------------------------------------------------------------
				//Initialization of CAN1 Module and Enabling of CAN1 Interrupts
//--------------------------------------------------------------------------------------------------------------------

 
 C1CTRLbits.CANCKS = 0;			// Select the CAN Master Clock . It is equal to 4Fcy here. 
								// (Fcy=4MHz)

 C1CFG1bits.SJW=00;				//Synchronized jump width time is 1 x TQ when SJW is equal to 00
 
 C1CFG1bits.BRP = BRP_VAL;		//((FCY/(2*NTQ*BITRATE))-1) 	

 C1CFG2 = CAN_TIMING_CONFIG2;	// See canbus config for settings

///Interrupt Section of CAN Peripheral

 C1INTF = 0;					//Reset all The CAN Interrupts 
 IFS1bits.C1IF = 0;  			//Reset the Interrupt Flag status register
 C1INTE = 0x00FF;               //Enable interrupt sources
 IEC1bits.C1IE = 1;				//Enable the CAN1 Interrupt
};


//-----------------------------------------------------------------------------------------------------------------------
		// Send servo output control data to remote servo interface(s)
//-----------------------------------------------------------------------------------------------------------------------
void send_servo_outputs(void)
{
	if(C1TX0CONbits.TXREQ == 1)
	{
		return;
	}

	TxMsgCount = 1;

	C1TX0EID = 0x0001;				// First servo address
	C1TX0B1 = ServoOutBuff[0];
	C1TX0B2 = ServoOutBuff[1];
	C1TX0B3 = ServoOutBuff[2];
	C1TX0B4 = ServoOutBuff[3];

	C1TX0CONbits.TXREQ = 1;

	send_flight_data();		// May not belong here MJC
};


//-----------------------------------------------------------------------------------------------------------------------
		// Broadcast flight data on CANbus
//-----------------------------------------------------------------------------------------------------------------------
void send_flight_data()
{
	if(C1TX2CONbits.TXREQ == 1)	// If already transmitting data then return
	{
		return;
	}

	// Send some false data.  To be replaced by real data MJC
	if(TxDataCount == 0)
	{
		C1TX2EID = DATA_HEADINGS;
		C1TX2B1 = 0xAAAA;
		C1TX2B2 = 0xBBBB;
		C1TX2B3 = 0x5555;
		C1TX2B4 = 0x3333;
		C1TX2CONbits.TXREQ = 1;
	}
};


//-----------------------------------------------------------------------------------------------------------------------
		// Configure Receive buffer 0 and filter 1 for servo control
//-----------------------------------------------------------------------------------------------------------------------
void config_CAN1_RX_SERVO(void)
{
 // initializing the Receive Buffer 0
 CAN_SID_RX rx_sid;

 rx_sid.bb = 0;
 rx_sid.format.MSG_type 	= CANB_SID_TYP_CONTROL;
 rx_sid.format.MSG_source 	= CANB_SID_SOURCE_SERVO_IF;
 rx_sid.format.EXIDE 		= 1;

 C1RX0CON =  0x0000; 	// Receive Buffer Status Reset
 
 // Acceptance Mask Registers
 C1RXM0SID  = 0x1FFD; //All of SID must be correct and extended address
 C1RXM0EIDH = 0x0000; 
 C1RXM0EIDL = 0x0000;

//Initializing of Acceptance Filters
 C1RXF0SID 	= rx_sid.bb;	//CAN1 Receive Acceptance Filter0 SID
 C1RXF0EIDH = 0x0000; 	//CAN1 Receive Acceptace  Filter0 Extended Identifier high byte
 C1RXF0EIDL = 0x0000;	//CAN1 Receive Acceptance Filter0 Extended identifier low byte
};


//-----------------------------------------------------------------------------------------------------------------------
		// Configure Receive buffer 1 for data and filter 1 for servo control
//-----------------------------------------------------------------------------------------------------------------------
void config_CAN1_RX_COPRO()
{
 // initializing the Receive Buffer 1
 CAN_SID_RX rx_sid;

 rx_sid.bb = 0;
 rx_sid.format.MSG_type 	= CANB_SID_TYP_DATA;
 rx_sid.format.MSG_source 	= CANB_SID_SOURCE_COPROCESSOR;
 rx_sid.format.EXIDE 		= 1;

 C1RX1CON =  0x0000; 	// Receive Buffer Status Reset

 // Acceptance Mask Registers
 C1RXM1SID  = 0x1FFD; //All of SID must be correct and extended address
 C1RXM1EIDH = 0x0000;
 C1RXM1EIDL = 0x0000;

//Initializing of Acceptance Filters
 C1RXF2SID 	= rx_sid.bb;	//CAN1 Receive Acceptance Filter2 SID
 C1RXF2EIDH = 0x0000; 	//CAN1 Receive Acceptace  Filter2 Extended Identifier high byte
 C1RXF2EIDL = 0x0000;	//CAN1 Receive Acceptance Filter2 Extended identifier low byte
};


//-----------------------------------------------------------------------------------------------------------------------
		// Configure Transmit Register 0 for servo control data transmission
//-----------------------------------------------------------------------------------------------------------------------
void config_CAN1_TX_SERVO(void)
{
 CAN_SID_TX tx_sid;

 tx_sid.bb = 0;
 tx_sid.format.MSG_type 	= CANB_SID_TYP_CONTROL;
 tx_sid.format.MSG_source 	= CANB_SID_SOURCE_AUTOPILOT;
 tx_sid.format.TXIDE 		= 1;
  
 C1TX0CON = 0x0003;     	// High priority
 C1TX0SID = tx_sid.bb;		// SID
 C1TX0EID = 0x0001;     	// EID
 C1TX0DLC = 0x040; 			// 8 byte
};


//-----------------------------------------------------------------------------------------------------------------------
		// Configure Transmit Register 0 for flight data transmission
//-----------------------------------------------------------------------------------------------------------------------
void config_CAN1_TX_DATA(void)
{
 CAN_SID_TX tx_sid;

 tx_sid.bb = 0;
 tx_sid.format.MSG_type 	= CANB_SID_TYP_DATA;
 tx_sid.format.MSG_source 	= CANB_SID_SOURCE_AUTOPILOT;
 tx_sid.format.TXIDE 		= 1;

 C1TX2CON = 0x0001;             // Low Intermediate priority
 C1TX2SID = tx_sid.bb;          // SID
 C1TX2EID = 0x0000;             // EID                   
 C1TX2DLC = 0x0040;				// 8 bytes
};



void __attribute__((interrupt, no_auto_psv)) _C1Interrupt(void)
{
	switch (C1CTRLbits.ICODE) 
	{
		case 7: 
			C1INTFbits.WAKIF = 0; 
			break;
		case 6:
			if(C1RX0CONbits.FILHIT0 == 0)
			{
				switch(C1RX0EID)
				{
				case 1:
			        ServoInBuff[0] = C1RX0B1; 
			        ServoInBuff[1] = C1RX0B2;  	//Move the recieve data from Buffers to InData
			        ServoInBuff[2] = C1RX0B3;	
			        ServoInBuff[3] = C1RX0B4;
					break;
				case 2:
			        ServoInBuff[4] = C1RX0B1; 
			        ServoInBuff[5] = C1RX0B2;  	//Move the recieve data from Buffers to InData
			        ServoInBuff[6] = C1RX0B3;	
			        ServoInBuff[7] = C1RX0B4;
					receive_servo_inputs();				
					break;
				};
			};
			C1INTFbits.RX0IF = 0;
			C1RX0CONbits.RXFUL = 0;
			break;
		case 5:
			// Add code to parse recevied data here 
			// A braekpoint stop here breaks the software.  To investigate MJC
			C1INTFbits.RX1IF = 0; // Add code to read buffer 1
			C1RX1CONbits.RXFUL = 0;
			break;
		case 4:
			if(TxMsgCount == 1)
			{
				C1TX0EID = 0x0002;	// Second servo address
				C1TX0B1 = ServoOutBuff[4];
				C1TX0B2 = ServoOutBuff[5];
				C1TX0B3 = ServoOutBuff[6];
				C1TX0B4 = ServoOutBuff[7];
				TxMsgCount++;

				C1TX0CONbits.TXREQ = 1;
			}
			else
			{
				TxMsgCount = 0;
			};

			C1INTFbits.TX0IF = 0;
			break;
		case 3: 
			C1INTFbits.TX1IF = 0; 
			TxMsgCount = 0;
			break;
		case 2: 
			C1INTFbits.TX2IF = 0;
			// Send next data here MJC
//			TxDataCount++;		// Send N messages of junk for test only
//			if(TxDataCount < 10)
//			{
//				C1TX2CONbits.TXREQ = 1;
//			}
//			else
//			{
//				TxDataCount = 0;
//			}; 

//			TxDataCount = 0;
			break;
		case 1: 
			if(C1INTFbits.EWARN)
			{
				if(C1INTFbits.TXWAR)
				{
					ResetAllCAN1Tx();
					C1INTFbits.TXWAR = 0;
				}

				if(C1INTFbits.RXWAR)
				{
					ResetAllCAN1Rx();
					C1INTFbits.RXWAR = 0;
				}

				if(C1INTFbits.RXEP)
				{
					ResetAllCAN1Rx();
					C1INTFbits.RXEP = 0;
				}

				if(C1INTFbits.TXEP)
				{
					ResetAllCAN1Tx();
					C1INTFbits.TXEP = 0;
				}

				C1INTFbits.EWARN = 0;
			}
			else if(C1INTFbits.TXBO)
			{
				ResetAllCAN1Tx();
				C1INTFbits.TXBO = 0;
			}
			C1INTFbits.ERRIF = 0;

			break;
		case 0:
			if(C1INTFbits.IVRIF)
			{
				ResetAllCAN1Rx();
				C1INTFbits.IVRIF = 0;
			};
			break;
	}

	IFS1bits.C1IF = 0; // Clear CAN1 interrupt flag before returning
};

void ResetAllCAN1Tx()
{
	C1INTFbits.TX0IF = 0;
	C1INTFbits.TX1IF = 0;
	C1INTFbits.TX2IF = 0;
};


void ResetAllCAN1Rx()
{
	C1INTFbits.RX0IF = 0;
	C1INTFbits.RX1IF = 0;
	C1RX0CONbits.RXFUL = 0;
	C1RX1CONbits.RXFUL = 0;
	C1INTFbits.RX0OVR = 0;
	C1INTFbits.RX1OVR = 0;
};


// Code sharing with Interface board for testing.
#ifdef BOARD_TYPE

// Autopilot recieves, reformats and acts on servo data from CAN interface
void receive_servo_inputs()
{
    int index;
	int offset_index;
	for(index = 0; index < 8; index++)
	{
		offset_index = index+1;		// correct for array offsets

		if( (ServoInBuff[index] & 0x8000) == 0)
		{
			pwIn[offset_index] = 3000;
		}
		else
		{
			pwIn[offset_index] = ServoInBuff[index] & 0x7fff;
			if(offset_index == FAILSAFE_INPUT_CHANNEL)
			{
				if ( (pwIn[FAILSAFE_INPUT_CHANNEL] > FAILSAFE_INPUT_MIN) && (pwIn[FAILSAFE_INPUT_CHANNEL] < FAILSAFE_INPUT_MAX ) )
				{
					pulsesselin++ ;
					failSafePulses++ ;
				}
				else
				{
					pulsesselin = failSafePulses = 0 ;
					flags._.radio_on = 0;
					LED_GREEN = LED_OFF ;
				}
			};
		};
	};
};

// Move SetupOutputs here for CANbus overide.
#ifdef CANBUS_INTERFACE
void setupOutputs( void )
{
	PDC1 = pwOut[1] ;
	PDC2 = pwOut[2] ;
	PDC3 = pwOut[3] ;

	memcpy(ServoOutBuff, &pwOut[1], sizeof(ServoOutBuff));
	send_servo_outputs();
}
#endif

#endif

